#include <stdio.h>
#include <stdlib.h>
#include "nuc900_reg.h"
#include "wblib.h"
#include "usbd.h"
#include "nuc900_sic.h"

//#define TEST_SD0
//#define TEST_SD1
//#define TEST_MS0
//#define TEST_MS1
#define TEST_SM

#ifdef TEST_SM
#include "nuc900_gnand.h"
#include "nuc900_sic.h"

NDRV_T _nandDiskDriver0 = 
{
	nandInit0,
	nandpread0,
	nandpwrite0,
	nand_is_page_dirty0,
	nand_is_valid_block0,
	nand_ioctl,
	nand_block_erase0,
	nand_chip_erase0,
	0
};
NDISK_T *ptMassNDisk;
#endif

#ifdef ENABLE_DBUFF
DB_CTRL_T  _Mass_DBCtl = { 0, 0, 0, 0, 0, 0, 0 };
#endif


extern INT  MASS_IO_Init(void);
extern INT  MASS_IO_FLUSH(UINT8 lun);
extern INT  MASS_IO_Write(UINT8 lun, UINT32 nSectorNo, INT nSectorCnt, UINT8 *buff);
extern INT  MASS_IO_Read(UINT8 lun, UINT32 nSectorNo, INT nSectorCnt, UINT8 *buff);


/* Mass_Storage command base address */
__align(4096) UINT8 Mass_Block[MASS_BUFFER_SIZE];
#ifdef ENABLE_DBUFF
__align(4096) UINT8 Storage_Block[2][STORAGE_BUFFER_SIZE];
#else
__align(4096) UINT8 Storage_Block[STORAGE_BUFFER_SIZE];
#endif

#define Mass_Base_Addr  ((UINT32)&Mass_Block[0] | 0x80000000)
#define Storage_Base_Addr  ((UINT32)Storage_Block | 0x80000000)

#define	SizePerSector		512

UINT8 volatile Mass_LUN=0;
UINT8 volatile F_SD0_LUN=0xff;
UINT8 volatile F_SD1_LUN=0xff;
UINT8 volatile F_MS0_LUN=0xff;
UINT8 volatile F_MS1_LUN=0xff;
UINT8 volatile F_SM_LUN=0xff;

Disk_Par_Info DDB;

UINT8 volatile preventflag=0;
UINT8 volatile Bulk_First_Flag=0;
UINT8 volatile sd0_card_remove_flag=0;
UINT8 volatile sd0_card_Insert_flag=0;
UINT8 volatile sd1_card_remove_flag=0;
UINT8 volatile sd1_card_Insert_flag=0;
UINT8 volatile no_card_flag=0;
UINT8 errorFlagCBW=0;

int volatile gTotalSectors_SD0, gTotalSectors_SD1, gTotalSectors_SM;
int volatile gTotalSectors_MS0, gTotalSectors_MS1;
int volatile Card_STS;

void DeviceInsert_sd0(void);
void DeviceRemove_sd0(void);
void DeviceInsert_sd1(void);
void DeviceRemove_sd1(void);
UINT8 Flash_Identify(UINT8 tLUN);

// code = 12h, Inquiry
static UINT8 InquiryID[36] = {
0x00, 0x80, 0x00, 0x01, 0x1F, 0x00, 0x00, 0x00,
0x57, 0x39, 0x30, 0x50, 0x39, 0x31, 0x30, 0x20,
0x55, 0x53, 0x42, 0x20, 0x43, 0x61, 0x72, 0x64,
0x20, 0x52, 0x65, 0x61, 0x64, 0x65, 0x72, 0x20,
0x31, 0x2E, 0x30, 0x30 };

// code = 5Ah, Mode Sense
static UINT8 Mode_Page_01[12] = {
	0x01, 0x0A, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00,
	0x03, 0x00, 0x00, 0x00 };

static UINT8 Mode_Page_05[32] = {
	0x05, 0x1E, 0x13, 0x88, 0x08, 0x20, 0x02, 0x00,
	0x01, 0xF4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x05, 0x1E, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x01, 0x68, 0x00, 0x00 };

static UINT8 Mode_Page_1B[12] = {
	0x1B, 0x0A, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00 };

static UINT8 Mode_Page_1C[8] = {
	0x1C, 0x06, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00 };

static UINT8 Mode_Page[24] = {
	0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	//0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x02, 0x00, 0x1C, 0x0A, 0x80, 0x03,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 };

UINT8 volatile SenseKey = 0x00;
UINT8 volatile ASC= 0x00;
UINT8 volatile ASCQ=0x00;

typedef struct {
	UINT8	dCBWSignature[4]; 
	UINT8	dCBWTag[4];
	union
	{
		UINT8	c[4];
		UINT32	c32;
	} dCBWDataTferLh;         
	UINT8	bmCBWFlags;	
	UINT8	bCBWLUN;	
	UINT8	bCBWCBLength;
	UINT8	OP_Code;
	UINT8	LUN;
	union 
	{
		UINT8	d[4];
		__packed	UINT32	d32;
	} LBA;
	union
	{
		UINT8	f[4];
		__packed	UINT32	f32;
	} CBWCB_In; 
	UINT8	CBWCB_Reserved[6];
} CBW;

union {
	CBW		CBWD; 	
	UINT8	CBW_Array[31];
} CBW_In;     

typedef struct {
	UINT8	dCSWSignature[4];
	UINT8	dCSWTag[4];
	union
	{
		UINT8	g[4];
		UINT32	g32;
	} dCSWDataResidue;    
	UINT8	bCSWStatus;	
} CSW;

union {
	CSW		CSWD; 	
	UINT8	CSW_Array[13];
} CSW_In;


void MscReset(void)
{
	outpw(REG_USBD_EPA_RSP_SC,EP_HALT);
	outpw(REG_USBD_EPB_RSP_SC,EP_HALT);
	errorFlagCBW = 1;
	_usbd_haltep1 = HALT;	// in endpoint halt
	_usbd_haltep2 = HALT;	// in endpoint halt  

}

UINT8 validateCbw(void)
{
	if ((!((CBW_In.CBWD.dCBWSignature[0]==0x55)&&
		(CBW_In.CBWD.dCBWSignature[1]==0x53)&&
		(CBW_In.CBWD.dCBWSignature[2]==0x42)&&
		(CBW_In.CBWD.dCBWSignature[3]==0x43)))
		)
	{
		bulkonlycmd=0;
		MscReset();
		return 1;
	}
	else
		return 0;

}

void MassBulk(void)
{
	if (Bulk_First_Flag==0)
	{
		Bulk_First_Flag=1;
		ReceiveCBW(Mass_Base_Addr);
	}

#ifdef TEST_SD0
    if (sd0_card_Insert_flag==1)
    {
       Flash_Identify(F_SD0_LUN);
       sd0_card_Insert_flag=0;
    }
#endif

#ifdef TEST_SD1
    if (sd1_card_Insert_flag==1)
    {
       Flash_Identify(F_SD1_LUN);
       sd1_card_Insert_flag=0;
    }
#endif

    if (bulkonlycmd==1)
    {
        FshBuf2CBW();
        if (validateCbw())
		{
			return;
		}
        if ((CBW_In.CBWD.OP_Code == 0x2a) || (CBW_In.CBWD.OP_Code == 0xaa))
        {
            Wt10_Command();
            CSW2FshBuf();
        }
        else if ((CBW_In.CBWD.OP_Code == 0x28)|| (CBW_In.CBWD.OP_Code == 0xa8))
        {
            Rd10_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x12)
        {
            Inquiry_Command();
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x23)
        {
            RdFmtCap_Command();
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x25)
        {
            RdCurCap_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x5a)
        {
            ModeSense_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x1a)
        {
            ModeSense6_Command();
            CSW2FshBuf();
        }
        else if (CBW_In.CBWD.OP_Code == 0x00)
        {
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x1e)
        {
            if (CBW_In.CBW_Array[19]==1)
            {
                preventflag=1;
                SenseKey = 0x05;
                ASC= 0x24;
                ASCQ = 0x00;
            }
            else
                preventflag=0;
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x03)
        {
            ReqSen_Command();
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x2F)
        {
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x1B)
        {
            CSW2FshBuf();
        }else if (CBW_In.CBWD.OP_Code == 0x55)
        {
            ModeSel_Command();
            CSW2FshBuf();
        }
        else
        {
            SenseKey = 0x05;
            ASC= 0x24;
            ASCQ = 0x00;
            CSW2FshBuf(); 
        }
        bulkonlycmd=0;
    }

#ifdef ENABLE_DBUFF    
	if (_Mass_DBCtl.is_queued)
	{
		if (sysGetTicks(TIMER0) - _Mass_DBCtl.q_time_stamp > 10)
		{
       		Card_STS = MASS_IO_Write(_Mass_DBCtl.lun, _Mass_DBCtl.q_start_sec, 
       								_Mass_DBCtl.q_sec_cnt, 
       								(UINT8 *)((UINT32)Storage_Block[_Mass_DBCtl.q_idx] | 0x80000000));
			_Mass_DBCtl.is_queued = 0;
		}
	}
#endif
    
#ifdef TEST_SM
#ifdef MIO_CACHE
    MASS_IO_FLUSH(F_SM_LUN);
#endif
#endif
}

void FshBuf2CBW(void)
{
	UINT32 i,j;

	for (i = 0 ; i < 17 ; i++)
		CBW_In.CBW_Array[i] = readb(Mass_Base_Addr+i);

	j = Mass_Base_Addr+20;
	for (i = 17 ; i < 21 ; i++)
		CBW_In.CBW_Array[i] = readb(j--);

	j = Mass_Base_Addr+24;
	for (i = 21 ; i < 25 ; i++)
		CBW_In.CBW_Array[i] = readb(j--);

	for (i = 25 ; i < 31 ; i++)
		CBW_In.CBW_Array[i] = readb(Mass_Base_Addr+i);
} 

void CSW2FshBuf(void)
{
	UINT8 i;

	for (i = 0 ; i < 13 ; i++)
		CSW_In.CSW_Array[i] = 0x00;

	CSW_In.CSWD.dCSWSignature[0] = 0x55;
	CSW_In.CSWD.dCSWSignature[1] = 0x53;
	CSW_In.CSWD.dCSWSignature[2] = 0x42;
	CSW_In.CSWD.dCSWSignature[3] = 0x53;

	for (i = 0 ; i < 4 ; i++)          
		CSW_In.CSWD.dCSWTag[i] = CBW_In.CBWD.dCBWTag[i];

    writeb(Mass_Base_Addr+12,0x00);

    CSW_In.CSWD.bCSWStatus = preventflag;

    for (i = 0 ; i < 13 ; i++)
        writeb(Mass_Base_Addr+i,CSW_In.CSW_Array[i]);

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        if ((Card_STS==FMI_SD_WRITE_PROTECT)&&(CBW_In.CBWD.OP_Code == 0x2a))
        {
            writeb(Mass_Base_Addr+12,0x01);
        }
        if ((sd0_card_remove_flag==1)&&(CBW_In.CBWD.OP_Code == 0x00))
            writeb(Mass_Base_Addr+12,0x01);
        if (no_card_flag==1)
        {
            if((CBW_In.CBWD.OP_Code == 0x12)||(CBW_In.CBWD.OP_Code == 0x03))
                writeb(Mass_Base_Addr+12,0x00);
            else
                writeb(Mass_Base_Addr+12,0x01);
        }
    }
#endif

#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        if ((Card_STS==FMI_SD_WRITE_PROTECT)&&(CBW_In.CBWD.OP_Code == 0x2a))
        {
            writeb(Mass_Base_Addr+12,0x01);
        }
        if ((sd1_card_remove_flag==1)&&(CBW_In.CBWD.OP_Code == 0x00))
            writeb(Mass_Base_Addr+12,0x01);
        if (no_card_flag==1)
        {
            if((CBW_In.CBWD.OP_Code == 0x12)||(CBW_In.CBWD.OP_Code == 0x03))
                writeb(Mass_Base_Addr+12,0x00);
            else
                writeb(Mass_Base_Addr+12,0x01);
        }
    }
#endif

	SDRAM2USB_Bulk(Mass_Base_Addr, 0x0d);
	Bulk_First_Flag=0;
}

void Inquiry_Command(void)
{
    int i;
    UINT32 wd;

    wd = CBW_In.CBWD.dCBWDataTferLh.c32;
    for (i = 0 ; i < sizeof(InquiryID); i++)
        writeb(Mass_Base_Addr+i,InquiryID[i]);
    SDRAM2USB_Bulk(Mass_Base_Addr,wd);
}

void RdFmtCap_Command(void)
{
    int i;
    UINT32 rd, temp;
    UINT32 tmpval;
     
    tmpval=Mass_Base_Addr;
    for (i = 0 ; i < 36 ; i++)
        writeb(tmpval+i,0x00);
    writeb(tmpval+3,0x10);

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
		temp = gTotalSectors_SD0;
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
		temp = gTotalSectors_SD1;
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
		temp = gTotalSectors_MS0;
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
		temp = gTotalSectors_MS1;
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
		temp = gTotalSectors_SM;
#endif

    writeb(tmpval+4,*((UINT8 *)&temp+3));
    writeb(tmpval+5,*((UINT8 *)&temp+2));
    writeb(tmpval+6,*((UINT8 *)&temp+1));
    writeb(tmpval+7,*((UINT8 *)&temp+0));
    writeb(tmpval+8,0x02);
    writeb(tmpval+10,0x02);
    writeb(tmpval+12,*((UINT8 *)&temp+3));
    writeb(tmpval+13,*((UINT8 *)&temp+2));
    writeb(tmpval+14,*((UINT8 *)&temp+1));
    writeb(tmpval+15,*((UINT8 *)&temp+0));
    writeb(tmpval+18,0x02);

    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
    SDRAM2USB_Bulk(Mass_Base_Addr,rd);
}

void RdCurCap_Command(void)
{
    int i;
    UINT32 tmpval, temp;
    
    tmpval=Mass_Base_Addr;
    for (i = 0 ; i < 36 ; i++)
        writeb(tmpval+i,0x00);

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
		if (gTotalSectors_SD0 <= 0)
			temp = 0;
		else
			temp = gTotalSectors_SD0 - 1;
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
		if (gTotalSectors_SD1 <= 0)
			temp = 0;
		else
			temp = gTotalSectors_SD1 - 1;
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
		if (gTotalSectors_MS0 <= 0)
			temp = 0;
		else
			temp = gTotalSectors_MS0 - 1;
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
		if (gTotalSectors_MS1 <= 0)
			temp = 0;
		else
			temp = gTotalSectors_MS1 - 1;
    }
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
		if (gTotalSectors_SM <= 0)
			temp = 0;
		else
			temp = gTotalSectors_SM - 1;
    }
#endif

	writeb(tmpval, *((UINT8 *)&temp+3));
	writeb(tmpval+1, *((UINT8 *)&temp+2));
	writeb(tmpval+2, *((UINT8 *)&temp+1));
	writeb(tmpval+3, *((UINT8 *)&temp));
    writeb(tmpval+6,0x02);
    SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

void Rd10_Command(void)
{
    UINT32 rd,lba1;
     
    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
    lba1 = CBW_In.CBWD.LBA.d32;

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        if (no_card_flag==0)
			Card_STS = sicSdRead(0, lba1, (rd/SizePerSector), Storage_Base_Addr);
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        if (no_card_flag==0)
            Card_STS=sicSdRead(1, lba1, (rd/SizePerSector), Storage_Base_Addr);
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
        if (no_card_flag==0)
            Card_STS=sicMsdRead(0, lba1, (rd/SizePerSector), Storage_Base_Addr);
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
        if (no_card_flag==0)
            Card_STS=sicMsRead(1, lba1, (rd/SizePerSector), Storage_Base_Addr);
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif

#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
        if (no_card_flag==0)
        {
#ifdef ENABLE_DBUFF   
			if (_Mass_DBCtl.is_queued)
			{
       			Card_STS = MASS_IO_Write(_Mass_DBCtl.lun, _Mass_DBCtl.q_start_sec, 
       										_Mass_DBCtl.q_sec_cnt, 
       										(UINT8 *)((UINT32)Storage_Block[_Mass_DBCtl.q_idx] | 0x80000000));
				_Mass_DBCtl.is_queued = 0;
			}
#endif      
      
#ifdef MIO_CACHE
			Card_STS = MASS_IO_Read(F_SM_LUN, lba1, (rd/SizePerSector), (UINT8 *)Storage_Base_Addr);
#else        
            Card_STS = GNAND_read(ptMassNDisk, lba1, (rd/SizePerSector), (UINT8 *)Storage_Base_Addr);
#endif
		}
//sysprintf("read: status [%x]\n", Card_STS);
        SDRAM2USB_Bulk(Storage_Base_Addr,rd);
    }
#endif

    rd=0;
}


void Wt10_Command(void)
{
    UINT32 rd,lba1;
     
    rd = CBW_In.CBWD.dCBWDataTferLh.c32;
    lba1 = CBW_In.CBWD.LBA.d32;

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (no_card_flag==0)
        {
            Card_STS=sicSdWrite(0, lba1, (rd/SizePerSector), Storage_Base_Addr);
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                SenseKey = 0x07;
                ASC= 0x27;
                ASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (no_card_flag==0)
        {
            Card_STS=sicSdWrite(1, lba1, (rd/SizePerSector), Storage_Base_Addr);
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                SenseKey = 0x07;
                ASC= 0x27;
                ASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_MS0
    if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (no_card_flag==0)
        {
            Card_STS=sicMsWrite(0, lba1, (rd/SizePerSector), Storage_Base_Addr);
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                SenseKey = 0x07;
                ASC= 0x27;
                ASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_MS1
    if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
    {
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);
        if (no_card_flag==0)
        {
            Card_STS=sicMsWrite(1, lba1, (rd/SizePerSector), Storage_Base_Addr);
            if (Card_STS==FMI_SD_WRITE_PROTECT)
            {
                SenseKey = 0x07;
                ASC= 0x27;
                ASCQ = 0x00;
            }
        }
    }
#endif
#ifdef TEST_SM
    if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
    {
#ifdef ENABLE_DBUFF

        USB2SDRAM_Bulk_DB(F_SM_LUN, lba1, (rd/SizePerSector), rd);

#else
        USB2SDRAM_Bulk(Storage_Base_Addr,rd);

#ifdef MIO_CACHE
        Card_STS = MASS_IO_Write(F_SM_LUN, lba1, (rd/SizePerSector), (UINT8 *)Storage_Base_Addr);
#else
        Card_STS=GNAND_write(ptMassNDisk, lba1, (rd/SizePerSector), (UINT8 *)Storage_Base_Addr);
#endif

#endif
//sysprintf("write: status [%x]\n", Card_STS);
    }
#endif

    rd=0;
}

#if 0
void ModeSense10_Command(void)
{
	UINT8 i,j;

	for (i = 0 ; i < 8 ; i++)
		writeb(Mass_Base_Addr+i, 0x00);

	switch (CBW_In.CBWD.LBA.d[0])
	{
		case 0x01:
			writeb(Mass_Base_Addr,19);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_01[j]);
				i++;
			}
			break;

		case 0x05:
			writeb(Mass_Base_Addr, 39);
			i = 8;
			for (j = 0; j<32; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_05[j]);
				i++;
			}
			writeb(Mass_Base_Addr+12, DDB.NumHead);
			writeb(Mass_Base_Addr+13, DDB.NumSector);
			writeb(Mass_Base_Addr+16, (UINT8)(DDB.NumCyl >> 8));
			writeb(Mass_Base_Addr+17, (UINT8)(DDB.NumCyl & 0x00ff));
			break;

		case 0x1B:
			writeb(Mass_Base_Addr,19);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_1B[j]);
				i++;
			}
			break;

		case 0x1C:
			writeb(Mass_Base_Addr,15);
			i = 8;
			for (j = 0; j<8; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_1C[j]);
				i++;
			}
			break;

		case 0x3F:
			writeb(Mass_Base_Addr, 0x47);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_01[j]);
				i++;
			}

			for (j = 0; j<32; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_05[j]);
				i++;
			}

			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_1B[j]);
				i++;
			}

			for (j = 0; j<8; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_1C[j]);
				i++;
			}
			writeb(Mass_Base_Addr+24, DDB.NumHead);
			writeb(Mass_Base_Addr+25, DDB.NumSector);
			writeb(Mass_Base_Addr+28, (UINT8)(DDB.NumCyl >> 8));
			writeb(Mass_Base_Addr+29, (UINT8)(DDB.NumCyl & 0x00ff));
			break;

		default:
			SenseKey = 0x05;  //INVALID COMMAND
			ASC = 0x24;
			ASCQ = 0;
	}
	SDRAM2USB_Bulk(Mass_Base_Addr, CBW_In.CBWD.dCBWDataTferLh.c32);
}
#endif

void ModeSense6_Command(void)
{
	UINT8 i;

	for (i = 0; i<4; i++)
		writeb(Mass_Base_Addr+i,Mode_Page[i]);

	SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

void ReqSen_Command(void)
{
    UINT8 i;

    for (i = 0 ; i < 18 ; i++)
        writeb(Mass_Base_Addr+i,0x00);
    if (preventflag==1)
    {
        preventflag=0;
        writeb(Mass_Base_Addr,0x70);
    }
    else
        writeb(Mass_Base_Addr,0xf0);

#ifdef TEST_SD0
    if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
    {
        if (no_card_flag == 1)
        {
            SenseKey = 0x02;
            ASC= 0x3a;
            ASCQ = 0x00;
        }
        if (sd0_card_remove_flag==1)
        {
            SenseKey = 0x06;
            ASC = 0x28;
            ASCQ = 0x00;
            sd0_card_remove_flag=0;
        }
    }
#endif

#ifdef TEST_SD1
    if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
    {
        if (no_card_flag == 1)
        {
            SenseKey = 0x02;
            ASC= 0x3a;
            ASCQ = 0x00;
        }
        if (sd1_card_remove_flag==1)
        {
            SenseKey = 0x06;
            ASC = 0x28;
            ASCQ = 0x00;
            sd1_card_remove_flag=0;
        }
    }
#endif

    writeb(Mass_Base_Addr+2,SenseKey);
    writeb(Mass_Base_Addr+7,0x0a);
    writeb(Mass_Base_Addr+12,ASC);
    writeb(Mass_Base_Addr+13,ASCQ);
    SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
    SenseKey = 0;
    ASC= 0;
    ASCQ = 0;
}

void ModeSel_Command(void)
{
    USB2SDRAM_Bulk(Storage_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}

void ModeSense_Command(void)
{
    UINT8 i,j;
	UINT8 NumHead,NumSector,NumCyl;

	for (i = 0 ; i < 8 ; i++)
		writeb(Mass_Base_Addr+i, 0x00);

/*
	if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
	{
		if (_fmi_bIsSDWriteProtect == TRUE)
			writeb(Mass_Base_Addr+3,0x80);
	}
	if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
	{
		if (_fmi_bIsSDWriteProtect == TRUE)
			writeb(Mass_Base_Addr+3,0x80);
	}
*/

	switch (CBW_In.CBWD.LBA.d[0])
	{
		case 0x01:
			writeb(Mass_Base_Addr,19);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_01[j]);
				i++;
			}
			break;

		case 0x05:
			writeb(Mass_Base_Addr, 39);
			i = 8;
			for (j = 0; j<32; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_05[j]);
				i++;
			}
			
			NumHead = 2;
			NumSector = 64;

#ifdef TEST_SD0
			if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
			NumCyl = gTotalSectors_SD0/128;
#endif
#ifdef TEST_SD1
			if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
			NumCyl = gTotalSectors_SD1/128;
#endif
#ifdef TEST_MS0
			if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
			NumCyl = gTotalSectors_MS0/128;
#endif
#ifdef TEST_MS1
			if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
			NumCyl = gTotalSectors_MS1/128;
#endif
#ifdef TEST_SM
			if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
			NumCyl = gTotalSectors_SM/128;
#endif
			writeb(Mass_Base_Addr+12,NumHead);
			writeb(Mass_Base_Addr+13,NumSector);
			writeb(Mass_Base_Addr+16,(UINT8)(NumCyl >> 8));
			writeb(Mass_Base_Addr+17,(UINT8)(NumCyl & 0x00ff));
			break;

		case 0x1B:
			writeb(Mass_Base_Addr,19);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_1B[j]);
				i++;
			}
			break;

		case 0x1C:
			writeb(Mass_Base_Addr,15);
			i = 8;
			for (j = 0; j<8; j++)
			{
				writeb(Mass_Base_Addr+i,Mode_Page_1C[j]);
				i++;
			}
			break;

		case 0x3F:
			writeb(Mass_Base_Addr, 0x47);
			i = 8;
			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_01[j]);
				i++;
			}

			for (j = 0; j<32; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_05[j]);
				i++;
			}

			for (j = 0; j<12; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_1B[j]);
				i++;
			}

			for (j = 0; j<8; j++)
			{
				writeb(Mass_Base_Addr+i, Mode_Page_1C[j]);
				i++;
			}
			
			NumHead = 2;
			NumSector = 64;

#ifdef TEST_SD0
			if (CBW_In.CBWD.bCBWLUN==F_SD0_LUN)
			NumCyl = gTotalSectors_SD0/128;
#endif
#ifdef TEST_SD1
			if (CBW_In.CBWD.bCBWLUN==F_SD1_LUN)
			NumCyl = gTotalSectors_SD1/128;
#endif
#ifdef TEST_MS0
			if (CBW_In.CBWD.bCBWLUN==F_MS0_LUN)
			NumCyl = gTotalSectors_MS0/128;
#endif
#ifdef TEST_MS1
			if (CBW_In.CBWD.bCBWLUN==F_MS1_LUN)
			NumCyl = gTotalSectors_MS1/128;
#endif
#ifdef TEST_SM
			if (CBW_In.CBWD.bCBWLUN==F_SM_LUN)
			NumCyl = gTotalSectors_SM/128;
#endif
			writeb(Mass_Base_Addr+24,NumHead);
			writeb(Mass_Base_Addr+25,NumSector);
			writeb(Mass_Base_Addr+28,(UINT8)(NumCyl >> 8));
			writeb(Mass_Base_Addr+29,(UINT8)(NumCyl & 0x00ff));
			break;

		default:
			SenseKey = 0x05;  //INVALID COMMAND
			ASC = 0x24;
			ASCQ = 0;
	}
	SDRAM2USB_Bulk(Mass_Base_Addr,CBW_In.CBWD.dCBWDataTferLh.c32);
}


UINT8 Flash_Init(void)
{
    F_SD0_LUN=0xff;
    F_SD1_LUN=0xff;
    F_MS0_LUN=0xff;
    F_MS1_LUN=0xff;
    F_SM_LUN=0xff;

#ifdef TEST_SD0
     F_SD0_LUN=Mass_LUN;
     Mass_LUN++;

	sicIoctl(SIC_SET_CALLBACK,FMI_SD_CARD0,(INT32)DeviceRemove_sd0,(INT32)DeviceInsert_sd0);
#endif

#ifdef TEST_SD1
     F_SD1_LUN=Mass_LUN;
     Mass_LUN++;

    sicIoctl(SIC_SET_CALLBACK,FMI_SD_CARD1,(INT32)DeviceRemove_sd1,(INT32)DeviceInsert_sd1);
#endif

#ifdef TEST_MS0
     F_MS0_LUN=Mass_LUN;
     Mass_LUN++;
#endif

#ifdef TEST_MS1
     F_MS1_LUN=Mass_LUN;
     Mass_LUN++;
#endif

#ifdef TEST_SM
     F_SM_LUN=Mass_LUN;
     Mass_LUN++;
#endif

    sicOpen();

#ifdef TEST_SD0
    if (!Flash_Identify(F_SD0_LUN))
        return 0;
#endif
#ifdef TEST_SD1
    if (!Flash_Identify(F_SD1_LUN))
        return 0;
#endif
#ifdef TEST_MS0
    if (!Flash_Identify(F_MS0_LUN))
        return 0;
#endif
#ifdef TEST_MS1
    if (!Flash_Identify(F_MS1_LUN))
        return 0;
#endif
#ifdef TEST_SM
    if (!Flash_Identify(F_SM_LUN))
        return 0;
#endif

    return 1;
}

#ifdef TEST_SD0
void DeviceInsert_sd0(void)
{
    UINT32 tmp=0x100000;
    
    no_card_flag=0;
    sd0_card_remove_flag=0;
    sd0_card_Insert_flag=1;
    while (tmp-->0);
}

void DeviceRemove_sd0(void)
{
    no_card_flag=1;
    sd0_card_remove_flag=1;
    sd0_card_Insert_flag=0;
}
#endif

#ifdef TEST_SD1
void DeviceInsert_sd1(void)
{
    UINT32 tmp=0x100000;
    
    no_card_flag=0;
    sd1_card_remove_flag=0;
    sd1_card_Insert_flag=1;
    while (tmp-->0);
}

void DeviceRemove_sd1(void)
{
    no_card_flag=1;
    sd1_card_remove_flag=1;
    sd1_card_Insert_flag=0;
}
#endif

UINT8 Flash_Identify(UINT8 tLUN)
{
#ifdef TEST_SD0
    if (tLUN==F_SD0_LUN)
    {
        gTotalSectors_SD0 = sicSdOpen(0);
        if (gTotalSectors_SD0 == FMI_NO_SD_CARD)
            no_card_flag=1;
        if (gTotalSectors_SD0 < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_SD1
    if (tLUN==F_SD1_LUN)
    {
        gTotalSectors_SD1 = sicSdOpen(1);
        if (gTotalSectors_SD1 == FMI_NO_SD_CARD)
            no_card_flag=1;
        if (gTotalSectors_SD1 < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_MS0
    if (tLUN==F_MS0_LUN)
    {
        gTotalSectors_MS0 = sicMsOpen(0);
        if (gTotalSectors_MS0 < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_MS1
    if (tLUN==F_MS1_LUN)
    {
        gTotalSectors_MS1 = sicMsOpen(1);
        if (gTotalSectors_MS1 < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif
#ifdef TEST_SM
    if (tLUN==F_SM_LUN)
    {
		{
		INT nSectorPerPage;
		ptMassNDisk = (NDISK_T *)malloc(sizeof(NDISK_T));
		if (ptMassNDisk == NULL)
			printf("malloc error!!\n");
	
		GNAND_InitNAND(&_nandDiskDriver0, ptMassNDisk, TRUE);
		//GNAND_Diagnostic(&_nandDiskDriver0, TRUE);

		nSectorPerPage = ptMassNDisk->nPageSize / 512;
       	gTotalSectors_SM = ptMassNDisk->nZone * (ptMassNDisk->nLBPerZone-1) * ptMassNDisk->nPagePerBlock * nSectorPerPage;
		}
        if (gTotalSectors_SM < 0)
        {
            SenseKey = 0x03;   //cannot read format 
            ASC= 0x30;
            ASCQ = 0x01;
            return 0;
        }
    }
#endif


    return 1;	      // success
}



